﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_BasicClass.cs
 * \brief	This file contains a class with basic mathematical methods for the
 * M44 SensorFusionAndMapping module.
 *  
 * This file contains all the basic mathematical methods needed by the DataMapping
 * module
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.10
 * \date      2010-02-04
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-07-05 \n
 *      First implementation of the M44_BasicClass.
 *      
 * - Version 0.02:   Antonio MATTA                          2009-07-16 \n
 *      Cell2Coordinates() method added
 *      
 * - Version 0.03:   Antonio MATTA                          2009-07-29 \n
 *      Local2FixedCoord() method added
 *      
 * - Version 0.04:   Antonio MATTA                          2009-08-31 \n
 *      ConfidenceAssignment() method added
 *      
 * - Version 0.05:   Antonio MATTA                          2009-08-31 \n
 *      QuadsNumber() method added
 *      
 * - Version 0.06:   Antonio MATTA                          2009-09-01 \n
 *      Functions ConfidenceAssignment, HeightGradientEstimation, 
 *      and FusionTemp2Local sent to different files.
 *   
 * - Version 0.07:   Antonio MATTA                          2009-09-28 \n
 *     Coordinates2CellHigh function modified.
 *     
 * - Version 0.08:   Antonio MATTA                          2010-01-12 \n
 *     Local2FixedCoord and Spherical2Cartesian functions modified.
 *     
 * - Version 0.09:   Antonio MATTA                          2010-01-13 \n
 *     Coordinates2Cell functions modified.
 * 
 * - Version 0.10: Thomas KOPFSTEDT                         2010-02-04 \n
 *   changes of class names due to cleanup in "interfaces.cs", and the 
 *   other interface classed to double names for the same class
 *   
 * - Version 0.11:   Antonio MATTA                          2010-02-15 \n
 *     MapCenterIndex modified to make it comply with 
 *     CSML indices and MapCenterIndexHigh function eliminated because 
 *     it is not longer needed.
 *     
 * - Version 0.12:   Antonio MATTA                          2010-02-16 \n
 *     MapCenterIndex modified. We are only going to get even Maps.
 *     
 * - Version 0.13:   Antonio MATTA                          2010-02-18 \n
 *     Coordinates outside of map warning message comment out.
 *     
 * - Version 0.14:   Antonio MATTA                          2010-02-18 \n
 *     Coordinates2Cell and Cell2Coordinates changed in order to make the
 *     generation of the maps right.
 *    
 * - Version 0.14:   Antonio MATTA                          2010-02-18 \n
 *     Cell2Coordinates changed in order to make the generation 
 *     of the HighResMap cell.
 *     
 * - Version 0.15:   Antonio MATTA                          2010-02-18 \n
 *     Cell2Coordinates changed in order to make the generation 
 *     of the HighResMap cell.
 *     
 * - Version 0.16:   Antonio MATTA & Luis VALDES            2010-03-03 \n
 *     Cell2Coordinates and Coordinates2Cell changed
 *     
 * - Version 0.17:  Luis VALDES                             2010-04-27 \n
 *      Function CheckHRMlimits input arguments changed
 * 
 * - Version 0.18:  Gonzalo RODRIGUEZ                       2010-07-13 \n
 *      HighResMap Transformation Functions added
 *     
 */
/* ######################################################################### */
using System;
using System.Collections.Generic;
using System.Text;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;
using Interfaces.Basis;

/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;

/*This is for using lists*/
using System.Collections;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_BasicClass
     * 
     *  \brief  This is the class that holds all the matemathical methods.
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_BasicClass
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static int[] Coordinates2Cell( cl_Map2D LowResMap,
         *                                             double[] CollisionPoint
         *                                           )
         * 
         *  \brief This function finds the correct cell in the matrix that represents 'Map'
         *   of the coordinates [x,y]
         *  
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] double[] CollisionPoint
         *
         */
        /* ------------------------------------------------------------------------- */
        public static int[] Coordinates2Cell( cl_Map2D LowResMap,
                                              double[] Pos
                                             )
        {
            int[] Cell = new int[2];
            int[] Cm = new int[2];

            /*We determine the center cell of the map*/
            Cm = MapCenterIndex(LowResMap);

            if (Pos.Length == 3)
            {
                Cell[1] = Cm[1] + (int)Math.Round((Pos[2] - LowResMap.Center.d_y) / (LowResMap.us_Scale));
            }
            else
            {
                Cell[1] = Cm[1] + (int)Math.Round((Pos[1] - LowResMap.Center.d_y) / (LowResMap.us_Scale));
                
            }
            
            Cell[0] = Cm[0] + (int)Math.Round((Pos[0] - LowResMap.Center.d_x) / (LowResMap.us_Scale));
            
            if ((Cell[0] < 0) || (Cell[0] > LowResMap.us_Nrows) || (Cell[1] < 0) || (Cell[1] > LowResMap.us_Ncols))
            {
                //Console.WriteLine("Coordinates are outside the map");
            }

            return Cell;
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public static int[] Cell2Coordinates(cl_Map2D LowResMap,int[] i_global)
        * 
        *   \brief % This function returns the associated coordinates of the cell [i, j] in the Map.
        *  
        *  \param[in] cl_Map2D LowResMap
        *  \param[in] int[] i_global
        *
        */
        /* ------------------------------------------------------------------------- */
        public static float[] Cell2Coordinates( cl_Map2D LowResMap,
                                              int[] i_global
                                            )
        {
            int[] Cm = new int[2];
            float[] ti_coordinates = new float[2];

            if ((i_global[0] < 1) || (i_global[0]) >= (LowResMap.us_Nrows) || (i_global[1] < 1) || (i_global[1] >= LowResMap.us_Ncols))
            {
                //Console.WriteLine("Coordinates are outside the map");
            }

            /*We determine the center cell of the map*/
            Cm = MapCenterIndex(LowResMap);

            ti_coordinates[0] = (float)(LowResMap.Center.d_x + LowResMap.us_Scale * (i_global[0] - Cm[0]));
            ti_coordinates[1] = (float)(LowResMap.Center.d_y + LowResMap.us_Scale * (i_global[1] - Cm[1]));

            return ti_coordinates;
        }


        #region HighResMapTransformationfunctions

        /* ------------------------------------------------------------------------- */
        /**	\fn public static int[] Coordinates2Cell_High( cl_Map2D HighResMap,
         *                                             double[] CollisionPoint
         *                                           )
         * 
         *  \brief This function finds the correct cell in the matrix that represents 'Map'
         *   of the coordinates [x,y]
         *  
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] double[] CollisionPoint
         *
         */
        /* ------------------------------------------------------------------------- */

        
        public static int[] Coordinates2Cell_High(cl_Map2D LowResMap,
                                              double[] Pos
                                             )
        {
            int[] Cell = new int[2];
            int[] Cm = new int[2];
          //  double Scale = (double)LowResMap.us_Scale / M44_SensorFusionAndMappingService.i_MapRes;
            //We determine the center cell of the map
            Cm = MapCenterIndex(LowResMap);

            if (Pos.Length == 3)
            {
                Cell[1] = Cm[1] + (int)Math.Round((Pos[2] - LowResMap.Center.d_y) / ((float)LowResMap.us_Scale/10));
            }
            else
            {
                Cell[1] = Cm[1] + (int)Math.Round((Pos[1] - LowResMap.Center.d_y) / ((float)LowResMap.us_Scale / 10));

            }

            Cell[0] = Cm[0] + (int)Math.Round((Pos[0] - LowResMap.Center.d_x) / ((float)LowResMap.us_Scale / 10));

            if ((Cell[0] < 0) || (Cell[0] > LowResMap.us_Nrows) || (Cell[1] < 0) || (Cell[1] > LowResMap.us_Ncols))
            {
                //Console.WriteLine("Coordinates are outside the map");
            }

            return Cell;
        }
         

        /* ------------------------------------------------------------------------- */
        /**	\fn public static int[] Cell2Coordinates_High(cl_Map2D LowResMap,int[] i_global)
        * 
        *   \brief % This function returns the associated coordinates of the cell [i, j] in the Map.
        *  
        *  \param[in] cl_Map2D LowResMap
        *  \param[in] int[] i_global
        *
        */
        /* ------------------------------------------------------------------------- */

        
        public static float[] Cell2Coordinates_High(cl_Map2D LowResMap,
                                              int[] i_global
                                            )
        {
            int[] Cm = new int[2];
            float[] ti_coordinates = new float[2];

            if ((i_global[0] < 1) || (i_global[0]) >= (LowResMap.us_Nrows) || (i_global[1] < 1) || (i_global[1] >= LowResMap.us_Ncols))
            {
                //Console.WriteLine("Coordinates are outside the map");
            }

            //We determine the center cell of the map
            Cm = MapCenterIndex(LowResMap);

            ti_coordinates[0] = (float)(LowResMap.Center.d_x + (float)LowResMap.us_Scale / 10 * (i_global[0] - Cm[0]));
            ti_coordinates[1] = (float)(LowResMap.Center.d_y + (float)LowResMap.us_Scale / 10 * (i_global[1] - Cm[1]));

            return ti_coordinates;
        }
        
        
        #endregion



        /* ------------------------------------------------------------------------- */
        /**	\fn public static int[] MapCenterIndex( cl_Map2D Map )
         * 
         *  \brief This function gives the [i,j]-index of the center cell of a map.
         *   This function is necessary as during the creation phase of a map, the
         *   coordinates of its center not always are available.
         *  
         *  \param[in]  cl_Map2D LowResMap
         *  \param[out] int[2] Index of the Center of the Map
         *
         */
        /* ------------------------------------------------------------------------- */
        public static int[] MapCenterIndex(cl_Map2D Map)
        {
            
            int[] Cm = new int[2];
          
            Cm[0] = (Map.us_Nrows / 2)-1;
            Cm[1] = (Map.us_Ncols / 2);
            
            return Cm;
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public static int[] CheckHRMlimits(cl_Map2D LowResMap,
                                                   int[] i_global,
                                                   ushort us_Nrows,
                                                   ushort us_Ncols,
                                                   short s_n)
         * 
         *  \brief This function evaluates the limits of the HighResMap to not 
         *         exceed the global map.
         *  
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] int[] i_global
         *  \param[in] ushort us_Nrows
         *  \param[in] ushort us_Ncols
         *  \param[in] short s_n
         *
         */
        /* ------------------------------------------------------------------------- */
        public static int[] CheckHRMlimits(cl_Map2D LowResMap,
                                           int[] i_global,
                                           ushort us_Nrows,
                                           ushort us_Ncols,
                                           short s_n)
        {
            int i_init;
            int i_final;
            int j_init;
            int j_final;
            int[] i_NewCoordinates = new int[2];

            i_init = (int)(i_global[0] - (us_Nrows / 2 * 1 / s_n) - 1);
            i_final = (int)(i_global[0] + (us_Nrows / 2 * 1 / s_n));
            j_init = (int)(i_global[1] - (us_Ncols / 2 * 1 / s_n) - 1);
            j_final = (int)(i_global[1] + (us_Nrows / 2 * 1 / s_n));

            if (i_init < 1 && j_init >= 1 && j_final <= (LowResMap.us_Ncols-1))
            {
                i_global[0] = 1 + (us_Nrows / 2 * 1 / s_n);
            }
            else if (i_final > LowResMap.us_Nrows && j_init >= 1 && j_final <= LowResMap.us_Ncols)
            {
                i_global[0] = LowResMap.us_Nrows - (us_Nrows / 2 * 1 / s_n) - 1;
            }
            else if (j_init < 1 && i_init >= 1 && i_final <= LowResMap.us_Nrows)
            {
                i_global[1] = 1 + (us_Nrows / 2 * 1 / s_n);
            }
            else if (j_final > LowResMap.us_Ncols && i_init >= 1 && i_final <= LowResMap.us_Nrows)
            {
                i_global[1] = LowResMap.us_Ncols - (us_Nrows / 2 * 1 / s_n) - 1;
            }
            else if (i_init < 1 && j_init < 1)
            {
                i_global[0] = (us_Nrows / 2 * 1 / s_n) + 1;
                i_global[1] = 1 + (us_Nrows / 2 * 1 / s_n);
            }
            else if (i_final > LowResMap.us_Nrows && j_init < 1)
            {
                i_global[0] = LowResMap.us_Nrows - (us_Nrows / 2 * 1 / s_n) - 1;
                i_global[0] = 1 + (us_Nrows / 2 * 1 / s_n);
            }
            else if (i_final > LowResMap.us_Nrows && j_final > LowResMap.us_Ncols)
            {
                i_global[0] = LowResMap.us_Nrows - (us_Nrows / 2 * 1 / s_n) - 1;
                i_global[1] = LowResMap.us_Ncols - (us_Nrows / 2 * 1 / s_n) - 1;
            }
            else if (i_init < 1 && j_final > LowResMap.us_Ncols)
            {
                i_global[0] = (us_Nrows / 2 * 1 / s_n);
                i_global[1] = LowResMap.us_Ncols - (us_Nrows / 2 * 1 / s_n) - 1;
            }


            i_NewCoordinates[0] = i_global[0];
            i_NewCoordinates[1] = i_global[1];
            return i_NewCoordinates;
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public static double[,] Spherical2Cartesian( float rho,
         *                                                  float sigma,
         *                                                  float alpha
         *                                                )
         * 
         *  \brief Function which calculates the cartesian coordinates of the point impacted 
         *  by the laser. The cartesian coordinates are calculed into the laser reference system.
         *  
         *  rho is the laser distance measure for each angle.
         *  sigma is the pitch angle in radians of the laser at every moment
         *  alpha is the azimuth angle in radians of the laser at every moment
         *  
         *  \param[in] float rho
         *  \param[in] float sigma
         *  \param[in] float alpha
         *
         */
        /* ------------------------------------------------------------------------- */
        public static double[,] Spherical2Cartesian(float rho,
                                                    float sigma,
                                                    float alpha
                                                   )
        {

            double[,] f_CartesianLocalCoord = new double[3, 1];
           
            f_CartesianLocalCoord[0, 0] = (rho * Math.Cos(sigma) * Math.Cos(alpha));
            f_CartesianLocalCoord[1, 0] = -(rho * Math.Sin(sigma) * Math.Cos(alpha));
            f_CartesianLocalCoord[2, 0] = -(rho * Math.Sin(alpha));

            return f_CartesianLocalCoord;
        }

        /* ------------------------------------------------------------------------- */
        /**	public static double[] Local2FixedCoord( float[] CartesianLocalCoord,
         *                                          cl_RobotStateEstimate r_RobotStateEstimate,
         *                                          cl_SensorData Sensors
         *                                         )
         * 
         *  \brief CartesianLocalCoord is the name of the laser measure vector into his own 
         *         reference system.
         *         
         * 
         *         phi,theta,psi: Euler angles of the robot respect to fixed system.
         *         phi is a spin respect X
         *         theta is a spin respect Y
         *         RobotPos(1), RobotPos(2), RobotPos(3) coordinates of the robot
         *         mobile system respect to the fixed sytem
         *  
         *  \param[in] float[] CartesianLocalCoord
         *  \param[in] cl_RobotStateEstimate r_RobotStateEstimate
         *  \param[in] cl_SensorData Sensors
         *
         */
        /* ------------------------------------------------------------------------- */
        public static double[] Local2FixedCoord(double[,] CartesianLocalCoord,
                                                cl_RobotGPSINSState r_RobotStateEstimate,
                                                cl_SensorData Sensors,
                                                cl_Orientation3D orientation
                                               )
        {
            double[] f_CartesianLocalCoord = new double[3];
            double[] RobotPos = new double[3];
            double phi;
            double theta;
            double psi;
            xna.Matrix Rot12 = new xna.Matrix();
            xna.Matrix Rot34 = new xna.Matrix();
            xna.Matrix T12 = new xna.Matrix();
            xna.Matrix T23 = new xna.Matrix();
            xna.Matrix CartesianLocalMatrix = new xna.Matrix();
            xna.Matrix ResultMatrix = new xna.Matrix();
            cl_Pos3D_double tf_3DLRFPosition = new cl_Pos3D_double();

            /*First we do a coordinates change to transform global map*/
            /*coordinates to local map*/
            RobotPos[0] = r_RobotStateEstimate.RobotEstimate.td_Pos[0];
            RobotPos[1] = r_RobotStateEstimate.RobotEstimate.td_Pos[1];
            RobotPos[2] = r_RobotStateEstimate.RobotEstimate.td_Pos[2];
            
            tf_3DLRFPosition.d_x = Sensors.Position.d_x;
            tf_3DLRFPosition.d_y = Sensors.Position.d_y;
            tf_3DLRFPosition.d_z = Sensors.Position.d_z;

            //phi = r_RobotStateEstimate.RobotEstimate.td_Attitude[0] + orientation.d_phi;
            //theta = r_RobotStateEstimate.RobotEstimate.td_Attitude[1] + orientation.d_chi;
            //psi = r_RobotStateEstimate.RobotEstimate.td_Attitude[2] + orientation.d_psi;

            phi = r_RobotStateEstimate.RobotEstimate.td_Attitude[0];
            theta = r_RobotStateEstimate.RobotEstimate.td_Attitude[1];
            psi = r_RobotStateEstimate.RobotEstimate.td_Attitude[2];

            /*Rot12 Matrix*/
            /*First column*/
            Rot12.M11 = (float)(Math.Cos(psi) * Math.Cos(theta));
            Rot12.M21 = (float)(Math.Sin(psi) * Math.Cos(theta));
            Rot12.M31 = (float)(-Math.Sin(theta));
            Rot12.M41 = 0;

            /*Second column*/
            Rot12.M12 = (float)(Math.Cos(psi) * Math.Sin(theta) * Math.Sin(phi) - Math.Sin(psi) * Math.Cos(phi));
            Rot12.M22 = (float)(Math.Sin(psi) * Math.Sin(theta) * Math.Sin(phi) + Math.Cos(psi) * Math.Cos(phi));
            Rot12.M32 = (float)(Math.Cos(theta) * Math.Sin(phi));
            Rot12.M42 = 0;

            /*Third column*/
            Rot12.M13 = (float)(Math.Cos(psi) * Math.Sin(theta) * Math.Cos(phi) + Math.Sin(psi) * Math.Sin(phi));
            Rot12.M23 = (float)(Math.Sin(psi) * Math.Sin(theta) * Math.Cos(phi) - Math.Cos(psi) * Math.Sin(phi));
            Rot12.M33 = (float)(Math.Cos(theta) * Math.Cos(phi));
            Rot12.M43 = 0;

            /*Fourth column*/
            Rot12.M14 = 0;
            Rot12.M24 = 0;
            Rot12.M34 = 0;
            Rot12.M44 = 1;

            /*T12 Matrix*/
            /*First column*/
            T12.M11 = 1;
            T12.M21 = 0;
            T12.M31 = 0;
            T12.M41 = 0;

            /*Second column*/
            T12.M12 = 0;
            T12.M22 = 1;
            T12.M32 = 0;
            T12.M42 = 0;

            /*Third column*/
            T12.M13 = 0;
            T12.M23 = 0;
            T12.M33 = 1;
            T12.M43 = 0;

            /*Fourth column*/
            T12.M14 = (float)RobotPos[0];
            T12.M24 = (float)RobotPos[1];
            T12.M34 = (float)RobotPos[2];
            T12.M44 = 1;

            /*T23 Matrix*/
            /*First column*/
            T23.M11 = 1;
            T23.M21 = 0;
            T23.M31 = 0;
            T23.M41 = 0;

            /*Second column*/
            T23.M12 = 0;
            T23.M22 = 1;
            T23.M32 = 0;
            T23.M42 = 0;

            /*Third column*/
            T23.M13 = 0;
            T23.M23 = 0;
            T23.M33 = 1;
            T23.M43 = 0;

            /*Fourth column*/
            T23.M14 = (float)tf_3DLRFPosition.d_x;
            T23.M24 = (float)tf_3DLRFPosition.d_y;
            T23.M34 = (float)tf_3DLRFPosition.d_z;
            T23.M44 = 1;

            /*Rot12 Matrix*/

            /*First column*/
            phi=orientation.d_phi;
            theta = orientation.d_chi;
            psi = orientation.d_psi;

            Rot34.M11 = (float)(Math.Cos(psi) * Math.Cos(theta));
            Rot34.M21 = (float)(Math.Sin(psi) * Math.Cos(theta));
            Rot34.M31 = (float)(-Math.Sin(theta));
            Rot34.M41 = 0;

            /*Second column*/
            Rot34.M12 = (float)(Math.Cos(psi) * Math.Sin(theta) * Math.Sin(phi) - Math.Sin(psi) * Math.Cos(phi));
            Rot34.M22 = (float)(Math.Sin(psi) * Math.Sin(theta) * Math.Sin(phi) + Math.Cos(psi) * Math.Cos(phi));
            Rot34.M32 = (float)(Math.Cos(theta) * Math.Sin(phi));
            Rot34.M42 = 0;

            /*Third column*/
            Rot34.M13 = (float)(Math.Cos(psi) * Math.Sin(theta) * Math.Cos(phi) + Math.Sin(psi) * Math.Sin(phi));
            Rot34.M23 = (float)(Math.Sin(psi) * Math.Sin(theta) * Math.Cos(phi) - Math.Cos(psi) * Math.Sin(phi));
            Rot34.M33 = (float)(Math.Cos(theta) * Math.Cos(phi));
            Rot34.M43 = 0;

            /*Fourth column*/
            Rot34.M14 = 0;
            Rot34.M24 = 0;
            Rot34.M34 = 0;
            Rot34.M44 = 1;

            /*CartesianLocal Matrix*/
            /*First column*/
            CartesianLocalMatrix.M11 = (float)CartesianLocalCoord[0, 0];
            CartesianLocalMatrix.M21 = (float)CartesianLocalCoord[1, 0];
            CartesianLocalMatrix.M31 = (float)CartesianLocalCoord[2, 0];
            CartesianLocalMatrix.M41 = 1;

            /*Second column*/
            CartesianLocalMatrix.M12 = 0;
            CartesianLocalMatrix.M22 = (float)CartesianLocalCoord[1, 0];
            CartesianLocalMatrix.M32 = 0;
            CartesianLocalMatrix.M42 = 0;

            /*Third column*/
            CartesianLocalMatrix.M13 = 0;
            CartesianLocalMatrix.M23 = 0;
            CartesianLocalMatrix.M33 = (float)CartesianLocalCoord[2, 0];
            CartesianLocalMatrix.M43 = 0;

            /*Fourth column*/
            CartesianLocalMatrix.M14 = 0.0f;
            CartesianLocalMatrix.M24 = 0.0f;
            CartesianLocalMatrix.M34 = 0.0f;
            CartesianLocalMatrix.M44 = 1;

            //ResultMatrix = xna.Matrix.Multiply(T12, xna.Matrix.Multiply(Rot12, xna.Matrix.Multiply(T23, CartesianLocalMatrix)));
            ResultMatrix = xna.Matrix.Multiply(T12, xna.Matrix.Multiply(Rot12, xna.Matrix.Multiply(T23, xna.Matrix.Multiply( Rot34,CartesianLocalMatrix))));
            f_CartesianLocalCoord[0] = ResultMatrix.M11;
            f_CartesianLocalCoord[1] = ResultMatrix.M21;
            f_CartesianLocalCoord[2] = ResultMatrix.M31;

            return f_CartesianLocalCoord;
        }
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */
